/*
 * Copyright 2010 SRI International
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <math.h>
#include <assert.h>
#include <float.h>
#include <limits.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#include "open_karto/Karto.h"

namespace karto
{

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

SensorManager *
SensorManager::GetInstance()
{
    static Singleton<SensorManager> sInstance;
    return sInstance.Get();
} // namespace SensorManager::GetInstance()

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

Object::Object()
    : m_pParameterManager(new ParameterManager())
{
}

Object::Object(const Name &rName)
    : m_Name(rName), m_pParameterManager(new ParameterManager())
{
}

Object::~Object()
{
    delete m_pParameterManager;
    m_pParameterManager = NULL;
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

Module::Module(const std::string &rName)
    : Object(rName)
{
}

Module::~Module()
{
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

Sensor::Sensor(const Name &rName)
    : Object(rName)
{
    m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager());
}

Sensor::~Sensor()
{
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

SensorData::SensorData(const Name &rSensorName)
    : Object(), m_StateId(-1), m_UniqueId(-1), m_SensorName(rSensorName), m_Time(0.0)
{
}

SensorData::~SensorData()
{
    forEach(CustomDataVector, &m_CustomData)
    {
        delete *iter;
    }

    m_CustomData.clear();
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void CellUpdater::operator()(kt_int32u index)
{
    kt_int8u *pDataPtr = m_pOccupancyGrid->GetDataPointer();
    kt_int32u *pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
    kt_int32u *pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();

    m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

std::ostream &operator<<(std::ostream &rStream, Exception &rException)
{
    rStream << "Error detect: " << std::endl;
    rStream << " ==> error code: " << rException.GetErrorCode() << std::endl;
    rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl;

    return rStream;
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan *pLocalizedRangeScan,
                                                           CoordinateConverter *pCoordinateConverter,
                                                           kt_bool ignoreThresholdPoints,
                                                           kt_bool flipY) const
{
    PointVectorDouble pointReadings;

    Pose2 scanPose = pLocalizedRangeScan->GetSensorPose();

    // compute point readings
    kt_int32u beamNum = 0;
    kt_double *pRangeReadings = pLocalizedRangeScan->GetRangeReadings();
    for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++)
    {
        kt_double rangeReading = pRangeReadings[i];

        if (ignoreThresholdPoints)
        {
            if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold()))
            {
                continue;
            }
        }
        else
        {
            rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold());
        }

        kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution();

        Vector2<kt_double> point;

        point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
        point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));

        if (pCoordinateConverter != NULL)
        {
            Vector2<kt_int32s> gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
            point.SetX(gridPoint.GetX());
            point.SetY(gridPoint.GetY());
        }

        pointReadings.push_back(point);
    }

    return pointReadings;
}

kt_bool LaserRangeFinder::Validate(SensorData *pSensorData)
{
    LaserRangeScan *pLaserRangeScan = dynamic_cast<LaserRangeScan *>(pSensorData);

    // verify number of range readings in LaserRangeScan matches the number of expected range readings
    // if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings())
    // {
    //     std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings()
    //               << " range readings, expected " << GetNumberOfRangeReadings() << std::endl;
    //     return false;
    // }

    return true;
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void ParameterManager::Clear()
{
    forEach(karto::ParameterVector, &m_Parameters)
    {
        delete *iter;
    }

    m_Parameters.clear();

    m_ParameterLookup.clear();
}

void ParameterManager::Add(AbstractParameter *pParameter)
{
    if (pParameter != NULL && pParameter->GetName() != "")
    {
        if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end())
        {
            m_Parameters.push_back(pParameter);

            m_ParameterLookup[pParameter->GetName()] = pParameter;
        }
        else
        {
            m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString());

            assert(false);
        }
    }
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

/*
  std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] =
  {
    "Custom",

    "Sick_LMS100",
    "Sick_LMS200",
    "Sick_LMS291",

    "Hokuyo_UTM_30LX",
    "Hokuyo_URG_04LX"
  };
   */
} // namespace karto
